#ifndef SCEPTER_LEROBOT_DRIVER__LEROBOT_HARDWARE_INTERFACE_HPP_
#define SCEPTER_LEROBOT_DRIVER__LEROBOT_HARDWARE_INTERFACE_HPP_

#include <memory>
#include <string>
#include <vector>

#include "hardware_interface/handle.hpp"
#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/system_interface.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "rclcpp_lifecycle/state.hpp"
#include "sensor_msgs/msg/joint_state.hpp"
#include "std_srvs/srv/set_bool.hpp"

namespace scepter_lerobot_driver
{

class LerobotHardwareInterface : public hardware_interface::SystemInterface
{
public:
  RCLCPP_SHARED_PTR_DEFINITIONS(LerobotHardwareInterface)

  hardware_interface::CallbackReturn on_init(
    const hardware_interface::HardwareInfo & info) override;

  hardware_interface::CallbackReturn on_configure(
    const rclcpp_lifecycle::State & previous_state) override;

  std::vector<hardware_interface::StateInterface> export_state_interfaces() override;

  std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;

  hardware_interface::CallbackReturn on_activate(
    const rclcpp_lifecycle::State & previous_state) override;

  hardware_interface::CallbackReturn on_deactivate(
    const rclcpp_lifecycle::State & previous_state) override;

  hardware_interface::return_type read(
    const rclcpp::Time & time, const rclcpp::Duration & period) override;

  hardware_interface::return_type write(
    const rclcpp::Time & time, const rclcpp::Duration & period) override;

private:
  // Parameters
  std::string robot_ip_;
  std::string config_file_;
  double communication_timeout_;
  
  // Joint info
  std::vector<std::string> joint_names_;
  std::vector<double> hw_commands_;
  std::vector<double> hw_positions_;
  std::vector<double> hw_velocities_;
  
  // ROS interfaces
  rclcpp::Node::SharedPtr node_;
  rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_state_subscriber_;
  rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr command_publisher_;
  rclcpp::Client<std_srvs::srv::SetBool>::SharedPtr enable_service_client_;
  
  // State tracking
  bool hardware_connected_;
  std::chrono::steady_clock::time_point last_read_time_;
  
  // Helper methods
  bool connect_to_robot();
  void joint_state_callback(const sensor_msgs::msg::JointState::SharedPtr msg);
  bool send_commands_to_robot();
};

}  // namespace scepter_lerobot_driver

#endif  // SCEPTER_LEROBOT_DRIVER__LEROBOT_HARDWARE_INTERFACE_HPP_